#include <cmath>

#include <QPointF>
#include <QDebug>

#include "computeparameter.h"
#include "roaditem.h"
#include "laneitem.h"
#include "commondefine.h"

#define LANE_RADIUS_LINE "inf"
#define MAX_ITERATE_NUM 30
#define NEIGHBOURHOOD pow(10,-2)
#define NEIGHBOURHOOD_DENOMINATOR_ZERO pow(10, -10)
#define NEIGHBOURHOOD_DENOMINATOR_DELTA pow(10, -20)
#define GEOMETRY_TYPE_LINE "line"
#define GEOMETRY_TYPE_ARC "arc"
#define GEOMETRY_TYPE_SPIRAL "spiral"
#define GEOMETRY_TYPE_POLY3 "poly3"
#define GEOMETRY_TYPE_PARAM_POLY3 "paramPoly3"
#define ITERATE_STEP_SIZE 0.001
#define SPIRAL_PARAMETER_A 0.001
#define SPIRAL_PARAMETER_B 0.0000015708
#define SPIRAL_PARAMETER_C 0.000000000524
#define SPIRAL_PARAMETER_D 0.00000000000041


ComputeParameter::ComputeParameter()
{
}

ComputeParameter::~ComputeParameter()
{
}

//计算直线的状态栏参数
StateParameter ComputeParameter::calculateLineParameter(GeometryAttributes *pGeometryAttributes, const QPointF &pointf)
{
    StateParameter lineParameter;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateLineParameter->" <<  "pGeometryAttributes<-" << "is NULL";
        return lineParameter;
    }

    calculateLineSTCoordinate(pGeometryAttributes, pointf, lineParameter);  //求解S和T坐标
    calculateCommonParameter(pGeometryAttributes, lineParameter);  //求解公共参数
    lineParameter.strLaneRadius = LANE_RADIUS_LINE;  //车道半径
    return lineParameter;
}

//计算圆弧的状态栏参数
StateParameter ComputeParameter::calculateArcParameter(GeometryAttributes *pGeometryAttributes, const QPointF &pointf)
{
    StateParameter arcParameter;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateArcParameter->" <<  "pGeometryAttributes<-" << "is NULL";
        return arcParameter;
    }

    calculateArcSTCoordinate(pGeometryAttributes, pointf, arcParameter);  //求解弧线S和T坐标
    calculateCommonParameter(pGeometryAttributes, arcParameter);  //求解公共参数
    double dLaneRadius = 1 / pGeometryAttributes->dGeometryStartCurvature;
    arcParameter.strLaneRadius = QString::number(dLaneRadius, 'f', 3).toStdString();  //车道半径
    return arcParameter;
}

//螺旋线状态栏参数求解
StateParameter ComputeParameter::calculateSpiralParameter(GeometryAttributes *pGeometryAttributes, const QPointF &pointf)
{
    StateParameter spiralParameter;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateSpiralParameter->" <<  "pGeometryAttributes<-" << "is NULL";
        return spiralParameter;
    }

    double dPointCurvature = calculateSpiralSTCoordinate(pGeometryAttributes, pointf, spiralParameter);  //求解螺旋线S和T坐标
    calculateCommonParameter(pGeometryAttributes, spiralParameter);  //求解公共参数

    if(fabs(dPointCurvature) < NEIGHBOURHOOD_DENOMINATOR_ZERO)
    {
        spiralParameter.strLaneRadius = LANE_RADIUS_LINE;
    }
    else
    {
        spiralParameter.strLaneRadius = QString::number(1 / dPointCurvature, 'f', 3).toStdString();  //车道半径
    }

    return spiralParameter;
}

//三次多项式曲线状态栏参数求解
StateParameter ComputeParameter::calculatePoly3Parameter(GeometryAttributes *pGeometryAttributes, const QPointF &pointf)
{
    StateParameter poly3Parameter;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculatePoly3Parameter->" << "pGeometryAttributes<-" << "is NULL";
        return poly3Parameter;
    }

    double dPointCurvature = calculatePoly3STCoordinate(pGeometryAttributes, pointf, poly3Parameter);  //求解三次多项式曲线S和T坐标
    calculateCommonParameter(pGeometryAttributes, poly3Parameter);  //求解公共参数

    if(fabs(dPointCurvature) < NEIGHBOURHOOD_DENOMINATOR_ZERO)
    {
        poly3Parameter.strLaneRadius = LANE_RADIUS_LINE;
    }
    else
    {
        poly3Parameter.strLaneRadius = QString::number(1 / dPointCurvature, 'f', 3).toStdString();  //车道半径
    }

    return poly3Parameter;
}

//求解直线S坐标和T坐标
void ComputeParameter::calculateLineSTCoordinate(GeometryAttributes *pGeometryAttributes, const QPointF &pointf, StateParameter &lineParameter)
{
    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateLineSTCoordinate->" <<  "pGeometryAttributes<-" << "is NULL";
        return;
    }

    double dMouseMovedXCoordinate = QPointF(pointf).rx();  //x
    double dMouseMovedYCoordinate = QPointF(pointf).ry();  //y
    double dSDistance;  //鼠标移动的点对应在基准线上的点与几何元素起点之间的距离(直线为欧几里得距离,其他为弧长)
    double dTDistance;  //T距离

    //判断直线几何元素是否垂直于X轴
    if(0 == cos(pGeometryAttributes->dGeometryHdg))
    {
        QPointF baselineCorrespondingPoint;
        baselineCorrespondingPoint.setX(pGeometryAttributes->dGeometryXCoordinate);
        baselineCorrespondingPoint.setY(dMouseMovedYCoordinate);
        dSDistance = sqrt(pow(baselineCorrespondingPoint.rx() - pGeometryAttributes->dGeometryXCoordinate, 2) + pow(baselineCorrespondingPoint.ry() - pGeometryAttributes->dGeometryYCoordinate, 2));
        dTDistance = sqrt(pow(dMouseMovedXCoordinate - baselineCorrespondingPoint.rx(), 2) + pow(dMouseMovedYCoordinate - baselineCorrespondingPoint.ry(), 2));
    }
    else
    {
        double dSlope = tan(pGeometryAttributes->dGeometryHdg);
        dTDistance = fabs(cos(pGeometryAttributes->dGeometryHdg)) * fabs(dSlope * dMouseMovedXCoordinate - dMouseMovedYCoordinate + pGeometryAttributes->dGeometryYCoordinate - dSlope * pGeometryAttributes->dGeometryXCoordinate);
        dSDistance = sqrt(pow(dMouseMovedXCoordinate - pGeometryAttributes->dGeometryXCoordinate, 2) + pow(dMouseMovedYCoordinate - pGeometryAttributes->dGeometryYCoordinate, 2) - pow(dTDistance, 2));
    }

    lineParameter.dSCoordinate = pGeometryAttributes->dGeometrySCoordinate + dSDistance;  //S坐标
    double dCrossProduct = cos(pGeometryAttributes->dGeometryHdg) * (dMouseMovedYCoordinate - pGeometryAttributes->dGeometryYCoordinate) - sin(pGeometryAttributes->dGeometryHdg) * (dMouseMovedXCoordinate - pGeometryAttributes->dGeometryXCoordinate);

    if(dCrossProduct < 0)
    {
        dTDistance = -dTDistance;
    }

    lineParameter.dTCoordinate = dTDistance;  //T坐标
}

//求解弧线S坐标和T坐标
void ComputeParameter::calculateArcSTCoordinate(GeometryAttributes *pGeometryAttributes, const QPointF &pointf, StateParameter &arcParameter)
{
    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateArcSTCoordinate->" <<  "pGeometryAttributes<-" << "is NULL";
        return;
    }

    double dMouseMovedXCoordinate = QPointF(pointf).rx();  //x
    double dMouseMovedYCoordinate = QPointF(pointf).ry();  //y

    //求解圆心坐标
    double dXCenterCoordinates = pGeometryAttributes->dGeometryXCoordinate - 1 / pGeometryAttributes->dGeometryStartCurvature * sin(pGeometryAttributes->dGeometryHdg);
    double dYCenterCoordinates = pGeometryAttributes->dGeometryYCoordinate + 1 / pGeometryAttributes->dGeometryStartCurvature * cos(pGeometryAttributes->dGeometryHdg);

    double dVectorProduct = (pGeometryAttributes->dGeometryXCoordinate - dXCenterCoordinates) * (dMouseMovedXCoordinate - dXCenterCoordinates) + (pGeometryAttributes->dGeometryYCoordinate - dYCenterCoordinates) * (dMouseMovedYCoordinate - dYCenterCoordinates);
    double dVectorMoldStartPoint = fabs(1 / pGeometryAttributes->dGeometryStartCurvature);
    double dVectorMoldMovePoint = sqrt(pow(dMouseMovedXCoordinate - dXCenterCoordinates, 2) + pow(dMouseMovedYCoordinate - dYCenterCoordinates, 2));
    double dAngle = acos(dVectorProduct / (dVectorMoldStartPoint * dVectorMoldMovePoint));

    double dCrossProduct = (pGeometryAttributes->dGeometryXCoordinate - dXCenterCoordinates) * (dMouseMovedYCoordinate - dYCenterCoordinates) - (dMouseMovedXCoordinate - dXCenterCoordinates) * (pGeometryAttributes->dGeometryYCoordinate - dYCenterCoordinates);

    if(dCrossProduct * pGeometryAttributes->dGeometryStartCurvature < 0)
    {
        dAngle += M_PI;
    }

    double dSDistance = fabs(dAngle * (1 / pGeometryAttributes->dGeometryStartCurvature));  //鼠标移动的点对应在基准线上的点与几何元素起点之间的距离(直线为欧几里得距离,其他为弧长)
    arcParameter.dSCoordinate = pGeometryAttributes->dGeometrySCoordinate + dSDistance;  //S坐标
    int iSgn = pGeometryAttributes->dGeometryStartCurvature / fabs(pGeometryAttributes->dGeometryStartCurvature);

    //两种情况等价
//    double dTDistance = iSgn * (fabs(1 / pGeometryAttributes->dGeometryStartCurvature) - dVectorMoldMovePoint);
    double dTDistance = -iSgn * dVectorMoldMovePoint + 1 / pGeometryAttributes->dGeometryStartCurvature;

    arcParameter.dTCoordinate = dTDistance;
}

//求解螺旋线S坐标和T坐标
double ComputeParameter::calculateSpiralSTCoordinate(GeometryAttributes *pGeometryAttributes, const QPointF &pointf, StateParameter &spiralParameter)
{
    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateSpiralSTCoordinate->" <<  "pGeometryAttributes<-" << "is NULL";
        return 0;
    }

    PointHdg pointHdg, pointHdgLeft;
    double dLeft, dRight, dMid;
    QPointF pointLeft;

    //求解dLeft和dRight的初值
    double dScaleFactor = sqrt(M_PI * pGeometryAttributes->dGeometryLength / fabs(pGeometryAttributes->dGeometryStartCurvature - pGeometryAttributes->dGeometryEndCurvature));
    double dParameterVarStart = fabs(pGeometryAttributes->dGeometryStartCurvature) * dScaleFactor / M_PI;
    double dParameterVarEnd = fabs(pGeometryAttributes->dGeometryEndCurvature) * dScaleFactor / M_PI;

    if(dParameterVarStart < dParameterVarEnd)
    {
        dLeft = dParameterVarStart;
        dRight = dParameterVarEnd;
    }
    else
    {
        dLeft = dParameterVarEnd;
        dRight = dParameterVarStart;
    }

    double dDistance = 0.0;

    //用迭代次数和距离进行精度上的确认
    for(int i = 0; i < MAX_ITERATE_NUM; ++i)
    {
        dMid = (dLeft + dRight) / 2;

        //带入重载函数,计算螺旋线上一个点的信息,而不是螺旋线的全部点信息
        pointHdg = geometrySpiralData(pGeometryAttributes->dGeometryXCoordinate, pGeometryAttributes->dGeometryYCoordinate, pGeometryAttributes->dGeometryHdg, pGeometryAttributes->dGeometryLength, pGeometryAttributes->dGeometryStartCurvature, pGeometryAttributes->dGeometryEndCurvature, dMid);
        pointHdgLeft = geometrySpiralData(pGeometryAttributes->dGeometryXCoordinate, pGeometryAttributes->dGeometryYCoordinate, pGeometryAttributes->dGeometryHdg, pGeometryAttributes->dGeometryLength, pGeometryAttributes->dGeometryStartCurvature, pGeometryAttributes->dGeometryEndCurvature, dLeft);
        dDistance = getMouseMovedPointDistance(pointHdg, pointf);

        if(dDistance < NEIGHBOURHOOD)
        {
            break;
        }

        //更新dLeft和dRight
        pointLeft = pointHdgLeft.qvecPointData[0];
        double dCrossProductLeftPoint = getPointCrossProduct(pointHdg, pointLeft);
        double dCrossProductMousePoint = getPointCrossProduct(pointHdg, pointf);

        if(dCrossProductLeftPoint * dCrossProductMousePoint > 0)
        {
            dRight = dMid;
        }
        else
        {
            dLeft = dMid;
        }
    }

    spiralParameter.dSCoordinate = pGeometryAttributes->dGeometrySCoordinate + pointHdg.qvecSData[0];
    spiralParameter.dTCoordinate = fabs(pGeometryAttributes->iLaneId) / pGeometryAttributes->iLaneId * sqrt(pow(pointHdg.qvecPointData[0].rx() - QPointF(pointf).rx(), 2) + pow(pointHdg.qvecPointData[0].ry() - QPointF(pointf).ry(), 2));

    double dCurvature = M_PI / dScaleFactor * dMid;

    if((pGeometryAttributes->dGeometryStartCurvature <= 0) && (pGeometryAttributes->dGeometryEndCurvature <= 0))
    {
        dCurvature = -dCurvature;
    }

    return dCurvature;
}

//求解三次多项式曲线S坐标和T坐标
double ComputeParameter::calculatePoly3STCoordinate(GeometryAttributes *pGeometryAttributes, const QPointF &pointf, StateParameter &poly3Parameter)
{
    double dCurvature = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculatePoly3STCoordinate->" << "pGeometryAttributes<-" << "is NULL";
        return dCurvature;
    }

    //坐标系转换
    QPointF pointUVCoordinateSystem = convertCoordinateSystem(pGeometryAttributes, pointf);
    double dMouseMovedUCoordinate = pointUVCoordinateSystem.rx();
    double dMouseMovedVCoordinate = pointUVCoordinateSystem.ry();

    //求距离最小的对应点
    double dUCoordinate = pGeometryAttributes->dGeometryLength / 2;
    double dUCoordinateNext = 0.0;

    //迭代
    for(int i = 0; i < MAX_ITERATE_NUM; ++i)
    {
        dUCoordinateNext = dUCoordinate - computeDistanceSquareFirstDerivative(pGeometryAttributes, pointUVCoordinateSystem, dUCoordinate) / computeDistanceSquareSecondDerivative(pGeometryAttributes, dMouseMovedVCoordinate, dUCoordinate);

        if(fabs(dUCoordinate - dUCoordinateNext) < NEIGHBOURHOOD_DENOMINATOR_DELTA)
        {
            break;
        }

        dUCoordinate = dUCoordinateNext;
    }

    if(dUCoordinate < 0)
    {
        dUCoordinate = 0;
    }

    //S坐标
    poly3Parameter.dSCoordinate = pGeometryAttributes->dGeometrySCoordinate + dUCoordinate;  //曲线的长度用求解出的对应点的U坐标取近似替代,这不是影响Offset的根本原因,根本原因可能是Item的边界,也很有可能不是

    //T坐标
    double dVCoordinate = getVCoordinate(pGeometryAttributes, dUCoordinate);
    double dTCoordinateFabs = sqrt(pow(dUCoordinate - dMouseMovedUCoordinate, 2) + pow(dVCoordinate - dMouseMovedVCoordinate, 2));
    double dVCoordinateFirstDerivative = getVCoordinateFirstDerivative(pGeometryAttributes, dUCoordinate);
    double dAlpha = atan(dVCoordinateFirstDerivative);
    double dCrossProduct = cos(dAlpha) * (dMouseMovedVCoordinate - dVCoordinate) - sin(dAlpha) * (dMouseMovedUCoordinate - dUCoordinate);
    double dTCoordinate = dTCoordinateFabs;

    if(dCrossProduct < 0)
    {
        dTCoordinate = -dTCoordinate;
    }

    poly3Parameter.dTCoordinate = dTCoordinate;

    //计算曲率
    dCurvature = getPoly3Curvature(pGeometryAttributes, dUCoordinate);

    return dCurvature;
}

//求解公共的参数
void ComputeParameter::calculateCommonParameter(GeometryAttributes *pGeometryAttributes, StateParameter &geometryParameter)
{
    if(geometryParameter.dSCoordinate < 0)
    {
        geometryParameter.dSCoordinate = 0;
    }

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateCommonParameter->" <<  "pGeometryAttributes<-" << "is NULL";
        return;
    }

    geometryParameter.strTrackId = pGeometryAttributes->strTrackId;  //道路ID
    geometryParameter.iLaneId = pGeometryAttributes->iLaneId;  //车道ID
    geometryParameter.strLaneType = pGeometryAttributes->strLaneType;  //车道类型

    double dLaneSectionSCoordinate = geometryParameter.dSCoordinate - pGeometryAttributes->dLaneSectionSCoordinate;
    double dCurrentLaneLeftMarkerWidth = 0.0;  //是一个向量(一维向量)

    double dLaneWidth;  //车道宽度
    int iLaneId = geometryParameter.iLaneId;
    Width widthLane = pGeometryAttributes->qvecLaneWidth[fabs(iLaneId) - 1];  //只存储全部左车道或者全部右车道
    //OpenDrive计算公式-->width = a + b * s + c * pow(s, 2) + d * pow(s, 3);
    dLaneWidth = widthLane.coefficients.a + widthLane.coefficients.b * dLaneSectionSCoordinate + widthLane.coefficients.c * pow(dLaneSectionSCoordinate, 2) + widthLane.coefficients.d * pow(dLaneSectionSCoordinate, 3);

    if(1 == fabs(iLaneId))
    {
        dCurrentLaneLeftMarkerWidth =  getLaneOffsetWidth(pGeometryAttributes, geometryParameter.dSCoordinate);
        dLaneWidth = dLaneWidth + dCurrentLaneLeftMarkerWidth;

        if(dLaneWidth < 0)
        {
            qDebug() << "0 And 1 Position Swap";
        }
    }
    else
    {
        double dLaneWidthTmp;

        for(int i = 0; i < fabs(iLaneId) - 1; ++i)
        {
            widthLane = pGeometryAttributes->qvecLaneWidth[i];
            dLaneWidthTmp = widthLane.coefficients.a + widthLane.coefficients.b * dLaneSectionSCoordinate + widthLane.coefficients.c * pow(dLaneSectionSCoordinate, 2) + widthLane.coefficients.d * pow(dLaneSectionSCoordinate, 3);
            dLaneWidthTmp = fabs(iLaneId) / iLaneId * dLaneWidthTmp;
            dCurrentLaneLeftMarkerWidth += dLaneWidthTmp;
        }
    }

    geometryParameter.dLaneWidth = fabs(dLaneWidth);  //车道宽度
    dLaneWidth = fabs(iLaneId) / iLaneId * dLaneWidth;  //变为向量
    geometryParameter.dLaneOffset = geometryParameter.dTCoordinate - dCurrentLaneLeftMarkerWidth - dLaneWidth / 2;  //车道偏移
    geometryParameter.dZCoordinate = getZCoordinate(pGeometryAttributes, geometryParameter.dSCoordinate, geometryParameter);  //Z坐标
}

//螺旋线求参数ST坐标,阈值距离求解
double ComputeParameter::getMouseMovedPointDistance(PointHdg &pointHdg, const QPointF &pointf)
{
    double dDistance;
    double dHdg = pointHdg.qvecHdgData[0];

    if(fabs(sin(dHdg)) < NEIGHBOURHOOD_DENOMINATOR_ZERO)
    {
        //螺旋线上的点的切角所形成的直线斜率为0
        dDistance = fabs(QPointF(pointf).rx() - pointHdg.qvecPointData[0].rx());
    }
    else if(fabs(cos(dHdg)) < NEIGHBOURHOOD_DENOMINATOR_ZERO)
    {
        //螺旋线上的点的切角所形成的直线斜率为不存在
        dDistance = fabs(QPointF(pointf).ry() - pointHdg.qvecPointData[0].ry());
    }
    else
    {
        double dHdgSlope = sin(dHdg) / cos(dHdg);  //螺旋线上的点的切角所形成的直线的斜率
        double dLineSlope = -1 / dHdgSlope;  //与切角形成直线垂直的直线的斜率
        double dXCoordinate = pointHdg.qvecPointData[0].rx();
        double dYCoordinate = pointHdg.qvecPointData[0].ry();
        double dConstant = dYCoordinate - dLineSlope * dXCoordinate;
        dDistance = fabs(dLineSlope * QPointF(pointf).rx() - QPointF(pointf).ry() + dConstant) / sqrt(pow(dLineSlope, 2) + 1);
    }

    return dDistance;
}

//计算给定向量与给定点之间的叉积
double ComputeParameter::getPointCrossProduct(PointHdg &pointHdg, const QPointF &pointf)
{
    double dXCoordinate = pointHdg.qvecPointData[0].rx();
    double dYCoordinate = pointHdg.qvecPointData[0].ry();
    double dHdg = pointHdg.qvecHdgData[0];
    double dVectorComponentX = QPointF(pointf).rx() - dXCoordinate;
    double dVectorComponentY = QPointF(pointf).ry() - dYCoordinate;
    return sin(dHdg) * dVectorComponentY + cos(dHdg) * dVectorComponentX;
}

//得到鼠标移动点的Z坐标
double ComputeParameter::getZCoordinate(GeometryAttributes *pGeometryAttributes, double dSCoordinate, StateParameter &stateParameter)
{
    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getZCoordinate->" <<  "pGeometryAttributes<-" << "is NULL";
        return 0;
    }

    QVector<SElevation> qvecElevation = pGeometryAttributes->qvecElevation;

    double dElevationZCoordinate = 0;

    if(!qvecElevation.empty())
    {
        int i;

        for(i = 0; i < qvecElevation.size(); ++i)
        {
            if(qvecElevation[i].s > dSCoordinate)
            {
                break;
            }
        }

        i -= 1;
        double dElevationRelativelySCoordinate = dSCoordinate - qvecElevation[i].s;
        dElevationZCoordinate = qvecElevation[i].coefficients.a + qvecElevation[i].coefficients.b * dElevationRelativelySCoordinate + qvecElevation[i].coefficients.c * pow(dElevationRelativelySCoordinate, 2) + qvecElevation[i].coefficients.d * pow(dElevationRelativelySCoordinate, 3);
    }

    //计算车道高度
    if((0 == pGeometryAttributes->height.inner.length()) || (0 == pGeometryAttributes->height.outer.length()))
    {
        return dElevationZCoordinate;
    }

    double dLaneProportionWidth = stateParameter.iLaneId / fabs(stateParameter.iLaneId) * stateParameter.dLaneOffset + stateParameter.dLaneWidth;  //车道比例宽度
    double dLaneHeightInner = std::stod(pGeometryAttributes->height.inner);
    double dLaneHeightOuter = std::stod(pGeometryAttributes->height.outer);
    double dLaneHeight = dLaneProportionWidth * (dLaneHeightOuter - dLaneHeightInner) / stateParameter.dLaneWidth + dLaneHeightInner;  //车道高度

    return dLaneHeight + dElevationZCoordinate;
}

//得到车道偏移
double ComputeParameter::getLaneOffsetWidth(GeometryAttributes *pGeometryAttributes, double dSCoordinate)
{
    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getLaneOffsetWidth->" <<  "pGeometryAttributes<-" << "is NULL";
        return 0;
    }

    double dLaneOffsetWidth = 0;
    QVector<SLaneOffset> qvecLaneOffset = pGeometryAttributes->qvecLaneOffset;

    if(!qvecLaneOffset.empty())
    {
        int i;

        for(i = 0; i < qvecLaneOffset.size(); ++i)
        {
            if(qvecLaneOffset[i].s > dSCoordinate)
            {
                break;
            }
        }

        i -= 1;
        double dLaneOffsetRelativelySCoordinate = dSCoordinate - qvecLaneOffset[i].s;
        dLaneOffsetWidth = qvecLaneOffset[i].coefficients.a + qvecLaneOffset[i].coefficients.b * dLaneOffsetRelativelySCoordinate + qvecLaneOffset[i].coefficients.c * pow(dLaneOffsetRelativelySCoordinate, 2) + qvecLaneOffset[i].coefficients.d * pow(dLaneOffsetRelativelySCoordinate, 3);
    }

    return dLaneOffsetWidth;
}

//根据road、lane两个item确定状态栏计算所需要的几何元素属性
void ComputeParameter::getGeometryAttributes(RoadItem *pRoadItem, LaneItem *pLaneItem, GeometryAttributes *&pGeometryAttributes)
{

    if(NULL ==pRoadItem || NULL == pLaneItem || NULL == pGeometryAttributes)
    {
        qDebug() << "getGeometryAttributes->" <<  "pGeometryAttributes | pRoadItem | pLaneItem<-" << "is NULL";
        return;
    }

    //道路ID
    pGeometryAttributes->strTrackId = pRoadItem->getId().toStdString();  //道路ID

    //车道ID
    pGeometryAttributes->iLaneId = pLaneItem->getId().toInt();  //车道ID

    //车道类型
    std::string strLaneType = pLaneItem->getLaneType().toStdString();
    pGeometryAttributes->strLaneType = strLaneType;  //车道类型

    //几何元素类型
    pGeometryAttributes->strGeometryType = pLaneItem->getGeometry().type;

    //几何元素S坐标
    pGeometryAttributes->dGeometrySCoordinate = pLaneItem->getGeometry().s;  //道路绝对S坐标

    //车道段S坐标
    pGeometryAttributes->dLaneSectionSCoordinate = pLaneItem->getLaneSection().s;  //道路绝对S坐标

    //几何元素X坐标
    pGeometryAttributes->dGeometryXCoordinate = pLaneItem->getGeometry().x;

    //几何元素Y坐标
    pGeometryAttributes->dGeometryYCoordinate = pLaneItem->getGeometry().y;

    //几何元素切角
    pGeometryAttributes->dGeometryHdg = pLaneItem->getGeometry().hdg;

    //几何元素长度
    pGeometryAttributes->dGeometryLength = pLaneItem->getGeometry().length;

    //几何元素曲率以及abcd参数
    if(0 == strcmp(pGeometryAttributes->strGeometryType.c_str(), GEOMETRY_TYPE_LINE))
    {
        //减少打印的类型,便于快速查找错误
    }
    else if(0 == strcmp(pGeometryAttributes->strGeometryType.c_str(), GEOMETRY_TYPE_ARC))
    {
        pGeometryAttributes->dGeometryStartCurvature = pLaneItem->getGeometry().extend.a;
    }
    else if(0 == strcmp(pGeometryAttributes->strGeometryType.c_str(), GEOMETRY_TYPE_SPIRAL))
    {
        pGeometryAttributes->dGeometryStartCurvature = pLaneItem->getGeometry().extend.a;
        pGeometryAttributes->dGeometryEndCurvature = pLaneItem->getGeometry().extend.b;
    }
    else if(0 == strcmp(pGeometryAttributes->strGeometryType.c_str(), GEOMETRY_TYPE_POLY3))
    {
        pGeometryAttributes->dGeometryParameterA = pLaneItem->getGeometry().extend.a;
        pGeometryAttributes->dGeometryParameterB = pLaneItem->getGeometry().extend.b;
        pGeometryAttributes->dGeometryParameterC = pLaneItem->getGeometry().extend.c;
        pGeometryAttributes->dGeometryParameterD = pLaneItem->getGeometry().extend.d;
    }
    else
    {
        qDebug() << "New Types Of Geometric Elements Have Appeared:" << pGeometryAttributes->strGeometryType.c_str();
    }

    //车道宽度参数
    getDirectionAllLaneWidth(pLaneItem, pGeometryAttributes);

    //道路车道偏移参数
    pGeometryAttributes->qvecLaneOffset = pRoadItem->getLaneOffsets();

    //车道高程参数
    pGeometryAttributes->qvecElevation = pRoadItem->getElevations();

    //车道高度参数
    pGeometryAttributes->height = pLaneItem->getLaneHeight();
}

//根据传进来的图元信息和坐标点,计算状态栏参数
StateParameter ComputeParameter::calculateParameter(RoadItem *pRoadItem, LaneItem *pLaneItem, const QPointF &pointf)
{
    StateParameter stateParameter;

    if(NULL == pRoadItem || NULL == pLaneItem)
    {
        qDebug() << "calculateParameter->" <<  "pRoadItem | pLaneItem<-" << "is NULL";
        return stateParameter;
    }

    GeometryAttributes *pGeometryAttributes = new GeometryAttributes;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "calculateParameter->" <<  "pGeometryAttributes<-" << "is NULL";
        return stateParameter;
    }

    getGeometryAttributes(pRoadItem, pLaneItem, pGeometryAttributes);

    std::string strGeometryType = pGeometryAttributes->strGeometryType;

    if(0 == strcmp(strGeometryType.c_str(), GEOMETRY_TYPE_LINE))
    {
        stateParameter = calculateLineParameter(pGeometryAttributes, pointf);
    }
    else if(0 == strcmp(strGeometryType.c_str(), GEOMETRY_TYPE_ARC))
    {
        stateParameter = calculateArcParameter(pGeometryAttributes, pointf);
    }
    else if(0 == strcmp(strGeometryType.c_str(), GEOMETRY_TYPE_SPIRAL))
    {
        stateParameter = calculateSpiralParameter(pGeometryAttributes, pointf);
    }
    else if(0 == strcmp(strGeometryType.c_str(), GEOMETRY_TYPE_POLY3))
    {
        stateParameter = calculatePoly3Parameter(pGeometryAttributes, pointf);
    }
//    else if(0 == strcmp(strGeometryType.c_str(), GEOMETRY_TYPE_PARAM_POLY3))
//    {
//        stateParameter = calculateParamPoly3Parameter(pGeometryAttributes, pointf);
//    }
    else
    {
        qDebug() << "New Types Of Geometric Elements Have Appeared:" << strGeometryType.c_str();
    }

    delete pGeometryAttributes;

    return stateParameter;
}

//通过比较车道ID的正负,确定车道的方向,得到该方向上的所有车道宽度的vector
void ComputeParameter::getDirectionAllLaneWidth(LaneItem *pLaneItem, GeometryAttributes *pGeometryAttributes)
{
    if(NULL == pLaneItem || NULL == pGeometryAttributes)
    {
        qDebug() << "getDirectionAllLaneWidth->" <<  "pGeometryAttributes | pLaneItem<-" << "is NULL";
        return;
    }

    //车道段车道宽度参数
    if(pGeometryAttributes->iLaneId > 0)
    {
        //左车道
        int iLaneNum = pLaneItem->getLaneSection().left.qvecLane.size();

        for(int i = 0; i < iLaneNum; ++i)
        {
            pGeometryAttributes->qvecLaneWidth.push_back(pLaneItem->getLaneSection().left.qvecLane[iLaneNum - i - 1].width);
        }
    }
    else
    {
        //右车道
        int iLaneNum = pLaneItem->getLaneSection().right.qvecLane.size();

        for(int i = 0; i < iLaneNum; ++i)
        {
            pGeometryAttributes->qvecLaneWidth.push_back(pLaneItem->getLaneSection().right.qvecLane[i].width);
        }
    }
}


//求解螺旋线上单个点的信息
PointHdg ComputeParameter::geometrySpiralData(double dXCoordinate, double dYCoordinate, double dHdg, double dLength, double dCurvatureStart, double dCurvatureEnd, double dParameterVar)
{
    PointHdg geometrySpiral;

    if(0 > (dCurvatureStart * dCurvatureEnd))
    {
        qDebug() << "There should be no such road";
        return geometrySpiral;
    }

    QVector<QPointF> qvecGeometrySpiral;
    QVector<double> qvecHdg;
    QVector<double> qvecS;
    QPointF qpointf;

    double dScaleFactorA = sqrt(M_PI * dLength / fabs(dCurvatureStart - dCurvatureEnd));
    double dStartVariable = fabs(dScaleFactorA * dCurvatureStart / M_PI);

    //中间mid点
    qpointf = subFunctionSpiral(dScaleFactorA, dParameterVar);
    qvecGeometrySpiral.push_back(qpointf);
    qvecHdg.push_back(M_PI / 2 * pow(dParameterVar, 2));
    qvecS.push_back(dScaleFactorA * dParameterVar);

    //螺旋线初始点
    qpointf = subFunctionSpiral(dScaleFactorA, dStartVariable);
    qvecGeometrySpiral.push_back(qpointf);
    qvecHdg.push_back(M_PI / 2 * pow(dStartVariable, 2));
    qvecS.push_back(dScaleFactorA * dStartVariable);

    //更新数据
    setPointAndHdg(dCurvatureStart, dCurvatureEnd, qvecGeometrySpiral, qvecHdg);

    double dSpinHdg = dHdg - qvecHdg[qvecHdg.size() - 1];  //旋转角

    //旋转
    spinPoint(dSpinHdg, qvecGeometrySpiral);

    //平移
    panPoint(dXCoordinate, dYCoordinate, qvecGeometrySpiral);

    //角度旋转
    for(int i = 0; i < qvecHdg.size(); ++i)
    {
        qvecHdg[i] = qvecHdg[i] + dSpinHdg;
    }

    geometrySpiral.qvecPointData = qvecGeometrySpiral;
    geometrySpiral.qvecHdgData = qvecHdg;
    geometrySpiral.qvecSData = qvecS;

    return geometrySpiral;
}

//求解螺旋线上单个点的信息子函数
QPointF ComputeParameter::subFunctionSpiral(double dScaleFactorA, double dVarValue)
{
    QPointF point;
    double dCurrentXCoordinate = 0;
    double dCurrentYCoordinate = 0;
    double dVar;

    for(dVar = ITERATE_STEP_SIZE; dVar <= dVarValue; dVar += ITERATE_STEP_SIZE)
    {
        double dTmp = M_PI * pow(dVar, 2) / 2;
        double dCosTmp = cos(dTmp);
        double dSinTmp = sin(dTmp);
        dCurrentXCoordinate = dCurrentXCoordinate + SPIRAL_PARAMETER_A * dCosTmp - SPIRAL_PARAMETER_B * dVar * dSinTmp - SPIRAL_PARAMETER_C * (dSinTmp + M_PI * pow(dVar, 2) * dCosTmp) - SPIRAL_PARAMETER_D * dVar * (3 * dCosTmp - M_PI * pow(dVar, 2) * dSinTmp);
        dCurrentYCoordinate = dCurrentYCoordinate + SPIRAL_PARAMETER_A * dSinTmp + SPIRAL_PARAMETER_B * dVar * dCosTmp + SPIRAL_PARAMETER_C * (dCosTmp - M_PI * pow(dVar, 2) * dSinTmp) - SPIRAL_PARAMETER_D * dVar * (3 * dSinTmp + M_PI * pow(dVar, 2) * dCosTmp);
    }

    point.setX(dScaleFactorA * dCurrentXCoordinate);
    point.setY(dScaleFactorA * dCurrentYCoordinate);
    return point;
}

//更新螺旋线上的数据点和切角的数值
void ComputeParameter::setPointAndHdg(double dCurvatureStart, double dCurvatureEnd, QVector<QPointF> &qvecGeometrySpiral, QVector<double> &qvecHdg)
{
    if((0 == dCurvatureStart) && (dCurvatureEnd > 0))
    {

    }
    else if((0 == dCurvatureStart) && (dCurvatureEnd < 0))
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = -qvecHdg[i];
            qvecGeometrySpiral[i].setY(-qvecGeometrySpiral[i].ry());
        }
    }
    else if((dCurvatureStart > 0) && (0 == dCurvatureEnd))
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = -qvecHdg[i] - M_PI;
            qvecGeometrySpiral[i].setY(-qvecGeometrySpiral[i].ry());
        }
    }
    else
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = qvecHdg[i] - M_PI;
        }
    }
}

//螺旋线的坐标旋转
void ComputeParameter::spinPoint(double dSpinHdg, QVector<QPointF> &qvecGeometrySpiral)
{
    double dCurrentXCoordinate, dCurrentYCoordinate, dNewXCoordinate, dnewYCoordinate, dAlpha, dBeta, dDistance;

    for(int i = 0; i < qvecGeometrySpiral.size(); ++i)
    {
        dCurrentXCoordinate = qvecGeometrySpiral[i].rx();
        dCurrentYCoordinate = qvecGeometrySpiral[i].ry();
        dDistance = sqrt(pow(dCurrentXCoordinate, 2) + pow(dCurrentYCoordinate, 2));

        if(0 == dCurrentXCoordinate)
        {
            if(dCurrentYCoordinate > 0)
            {
                dAlpha = M_PI / 2;
            }
            else if(dCurrentYCoordinate < 0)
            {
                dAlpha = -M_PI / 2;
            }
            else
            {
                dAlpha = 0;
            }
        }
        else
        {
            dAlpha = atan(dCurrentYCoordinate / dCurrentXCoordinate);
        }

        dBeta = dAlpha + dSpinHdg;
        dNewXCoordinate = dDistance * cos(dBeta);
        dnewYCoordinate = dDistance * sin(dBeta);
        qvecGeometrySpiral[i].setX(dNewXCoordinate);
        qvecGeometrySpiral[i].setY(dnewYCoordinate);
    }
}

//螺旋线的数据点坐标平移
void ComputeParameter::panPoint(double dXCoordinate, double dYCoordinate, QVector<QPointF> &qvecGeometrySpiral)
{
    double dPanXCoordinate = dXCoordinate - qvecGeometrySpiral[qvecGeometrySpiral.size() - 1].rx();
    double dPanYCoordinate = dYCoordinate - qvecGeometrySpiral[qvecGeometrySpiral.size() - 1].ry();

    for(int i = 0; i < qvecGeometrySpiral.size(); ++i)
    {
        qvecGeometrySpiral[i].setX(qvecGeometrySpiral[i].rx() + dPanXCoordinate);
        qvecGeometrySpiral[i].setY(qvecGeometrySpiral[i].ry() + dPanYCoordinate);
    }
}

//坐标系转换得到新的坐标点
QPointF ComputeParameter::convertCoordinateSystem(GeometryAttributes *pGeometryAttributes, const QPointF &pointf)
{
    QPointF pointUVCoordinateSystem;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "coordinateSystemConversion->" << "pGeometryAttributes<-" << "is NULL";
        return pointUVCoordinateSystem;
    }

    //鼠标点在xOy坐标系下的坐标
    double dMouseMovedXCoordinate = QPointF(pointf).rx();
    double dMouseMovedYCoordinate = QPointF(pointf).ry();

    //平移
    double dXCoordinateTmp = dMouseMovedXCoordinate - pGeometryAttributes->dGeometryXCoordinate;
    double dYCoordinateTmp = dMouseMovedYCoordinate - pGeometryAttributes->dGeometryYCoordinate;

    //旋转
    double dAlpha;

    if(0 == dXCoordinateTmp)
    {
        if(dYCoordinateTmp > 0)
        {
            dAlpha = M_PI / 2;
        }
        else
        {
            dAlpha = -M_PI / 2;
        }
    }
    else
    {
        dAlpha = atan(dYCoordinateTmp / dXCoordinateTmp);

        if(dXCoordinateTmp < 0)
        {
            dAlpha += M_PI;
        }
    }

    double dBeta = dAlpha - (pGeometryAttributes->dGeometryHdg - atan(pGeometryAttributes->dGeometryParameterB));
    double dLength = sqrt(pow(dXCoordinateTmp, 2) + pow(dYCoordinateTmp, 2));
    dXCoordinateTmp = dLength * cos(dBeta);
    dYCoordinateTmp = dLength * sin(dBeta);

    //平移
    pointUVCoordinateSystem.setX(dXCoordinateTmp);
    pointUVCoordinateSystem.setY(dYCoordinateTmp + pGeometryAttributes->dGeometryParameterA);

    return pointUVCoordinateSystem;
}

//距离平方函数一阶导函数
double ComputeParameter::computeDistanceSquareFirstDerivative(GeometryAttributes *pGeometryAttributes, QPointF &pointUVCoordinateSystem, double dUCoordinate)
{
    double dFirstDerivative = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "distanceSquareFirstDerivative->" << "pGeometryAttributes<-" << "is NULL";
        return dFirstDerivative;
    }

    //重新赋值,便于理解
    double a = pGeometryAttributes->dGeometryParameterA;
    double b = pGeometryAttributes->dGeometryParameterB;
    double c = pGeometryAttributes->dGeometryParameterC;
    double d = pGeometryAttributes->dGeometryParameterD;
    double x_u = pointUVCoordinateSystem.rx();
    double y_v = pointUVCoordinateSystem.ry();
    //见文档第四章三次多项式的状态栏参数计算
    dFirstDerivative = 3 * pow(d, 2) * pow(dUCoordinate, 5) +
                       5 * c * d * pow(dUCoordinate, 4) +
                       (4 * b * d + 2 * pow(c, 2)) * pow(dUCoordinate, 3) +
                       (3 * b * c + 3 * a * d - 3 * y_v * d) * pow(dUCoordinate, 2) +
                       (2 * a * c + pow(b, 2) + 1 - 2 * y_v * c) * dUCoordinate +
                       (a * b - x_u - y_v * b);

    return dFirstDerivative;
}

//距离平方函数二阶导函数
double ComputeParameter::computeDistanceSquareSecondDerivative(GeometryAttributes *pGeometryAttributes, double dMouseMovedVCoordinate, double dUCoordinate)
{
    double dSecondDerivative = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "distanceSquareSecondDerivative->" << "pGeometryAttributes<-" << "is NULL";
        return dSecondDerivative;
    }

    //重新赋值,便于理解
    double a = pGeometryAttributes->dGeometryParameterA;
    double b = pGeometryAttributes->dGeometryParameterB;
    double c = pGeometryAttributes->dGeometryParameterC;
    double d = pGeometryAttributes->dGeometryParameterD;
    double y_v = dMouseMovedVCoordinate;
    //见文档第四章三次多项式的状态栏参数计算
    dSecondDerivative = 15 * pow(d, 2) * pow(dUCoordinate, 4) +
                        20 * c * d * pow(dUCoordinate, 3) +
                        (12 * b * d + 6 * pow(c, 2)) * pow(dUCoordinate, 2) +
                        (6 * b * c + 6 * a * d - 6 * y_v * d) * dUCoordinate +
                        (2 * a * c + pow(b, 2) + 1 - 2 * y_v * c);

    return dSecondDerivative;
}

//得到变量U对应的变量V
double ComputeParameter::getVCoordinate(GeometryAttributes *pGeometryAttributes, double dUCoordinate)
{
    double dVCoordinate = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getVCoordinate->" << "pGeometryAttributes<-" << "is NULL";
        return dVCoordinate;
    }

    //重新赋值,便于理解
    double a = pGeometryAttributes->dGeometryParameterA;
    double b = pGeometryAttributes->dGeometryParameterB;
    double c = pGeometryAttributes->dGeometryParameterC;
    double d = pGeometryAttributes->dGeometryParameterD;
    //原方程为:v = a + b * u + c * u^2 + d * u^3;
    dVCoordinate = a + b * dUCoordinate + c * pow(dUCoordinate, 2) + d * pow(dUCoordinate, 3);

    return dVCoordinate;
}

//得到变量U对应的变量V的一阶导数
double ComputeParameter::getVCoordinateFirstDerivative(GeometryAttributes *pGeometryAttributes, double dUCoordinate)
{
    double dVCoordinateFirstDerivative = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getVCoordinateFirstDerivative->" << "pGeometryAttributes<-" << "is NULL";
        return dVCoordinateFirstDerivative;
    }

    //重新赋值,便于理解
    double b = pGeometryAttributes->dGeometryParameterB;
    double c = pGeometryAttributes->dGeometryParameterC;
    double d = pGeometryAttributes->dGeometryParameterD;
    //原方程为:v = a + b * u + c * u^2 + d * u^3;
    //求一阶导数为:dv / du = b + 2 * c * u + 3 * d * u^2;
    dVCoordinateFirstDerivative = b + 2 * c * dUCoordinate + 3 * d * pow(dUCoordinate, 2);

    return dVCoordinateFirstDerivative;
}

//得到变量U对应的变量V的二阶导数
double ComputeParameter::getVCoordinateSecondDerivative(GeometryAttributes *pGeometryAttributes, double dUCoordinate)
{
    double dVCoordinateSecondDerivative = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getVCoordinateSecondDerivative->" << "pGeometryAttributes<-" << "is NULL";
        return dVCoordinateSecondDerivative;
    }

    //重新赋值,便于理解
    double c = pGeometryAttributes->dGeometryParameterC;
    double d = pGeometryAttributes->dGeometryParameterD;
    //原方程为:v = a + b * u + c * u^2 + d * u^3;
    //求一阶导数为:dv / du = b + 2 * c * u + 3 * d * u^2;
    //求二阶导数为:d^2v / du^2 = 2 *c + 6 * d * u;
    dVCoordinateSecondDerivative = 2 * c + 6 * d * dUCoordinate;

    return dVCoordinateSecondDerivative;
}

//得到三次多项式在某一点(U,V)处的曲率(带正负值)
double ComputeParameter::getPoly3Curvature(GeometryAttributes *pGeometryAttributes, double dUCoordinate)
{
    double dCurvature = 0.0;

    if(NULL == pGeometryAttributes)
    {
        qDebug() << "getPoly3Curvature->" << "pGeometryAttributes<-" << "is NULL";
        return dCurvature;
    }

    double dVCoordinateFirstDerivative = getVCoordinateFirstDerivative(pGeometryAttributes, dUCoordinate);
    double dVCoordinateSecondDerivative = getVCoordinateSecondDerivative(pGeometryAttributes, dUCoordinate);
    dCurvature = dVCoordinateSecondDerivative / pow(1 + pow(dVCoordinateFirstDerivative, 2), 3 / 2);

    return dCurvature;
}
